import serial
from queue import Queue
import threading as td
import struct


def byte2float(bytes_list):
    """
    Convert bytes to float
    :param bytes_list: list of bytes in MSB
    :return: number in float
    """
    return struct.unpack('f', struct.pack('4B', *bytes_list))[0]


def float2byte(x):
    """
    Convert float to bytes
    :param x: number in float
    :return: list of bytes in MSB
    """
    return [_ for _ in struct.pack('f', x)]


def calc_check_byte(bytes_list):
    check = 0x00
    for byte in bytes_list:
        check += byte
    check %= 0x100
    return check


recv_dict = {
    b'\xac':    'Action Complete\n',
    b'\xbc':    'Serial Complete\n',
    b'\xcc':    'Sorting Complete\n',
}

send_dict = {
    'Reset':    0x00,
    'Move':     0x10,
    'SetC':     0x20,
    'Blue':     0xA0,
    'Green':    0xA1,
    'Pink':     0xA2,
    'Valve':    0xB0,
    'Pump':     0xB1,
    'Servo':    0xB2,
    'Hand':     0xC0,
}


class Com(serial.Serial):

    def __init__(self, port, baud, **kwargs):
        super().__init__(port, baud, **kwargs)
        # self.port = port
        # self.baud = int(baud)
        # self.com = None
        self.__recv_running = False
        self.recvBuffer = Queue(maxsize=20)
        self.recvThread = td.Thread()

    def activate_receive(self):
        self.__recv_running = True

        def receive_loop():
            while self.__recv_running:
                temp = self.read()
                if temp in recv_dict:
                    self.recvBuffer.put(recv_dict[temp])

        self.recvThread = td.Thread(target=receive_loop)
        self.recvThread.start()

    def deactivate_receive(self):
        self.__recv_running = False

    def clear_buffer(self):
        while not self.recvBuffer.empty():
            self.recvBuffer.get()

    def close(self):
        if self.isOpen():
            self.close()
            self.deactivate_receive()

    def send_reset(self):
        send_list = [0xAA]
        for _ in range(1, 10):
            send_list.append(0x00)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)

    def send_move(self, x, y):
        send_list = [0xAA, send_dict['Move']]
        send_list += float2byte(x)
        send_list += float2byte(y)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)

    def send_set_coordinate(self, x, y):
        send_list = [0xAA, send_dict['SetC']]
        send_list += float2byte(x)
        send_list += float2byte(y)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)

    def send_sorting(self, x, y, color):
        """

        :param x:
        :param y:
        :param color: 'blue', 'green', 'pink'
        :return:
        """
        cmd = 0x00
        if color == 'blue':
            cmd = send_dict['Blue']
        elif color == 'green':
            cmd = send_dict['Green']
        elif color == 'pink':
            cmd = send_dict['Pink']

        send_list = [0xAA, cmd]
        send_list += float2byte(x)
        send_list += float2byte(y)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)

    def send_adjust(self, item, status):
        cmd = 0x00
        if item == 'valve':
            cmd = send_dict['Valve']
        elif item == 'pump':
            cmd = send_dict['Pump']
        elif item == 'servo':
            cmd = send_dict['Servo']

        send_list = [0xAA, cmd]
        if status == 0:
            send_list.append(0x00)
        else:
            send_list.append(0x01)
        for _ in range(3, 10):
            send_list.append(0x00)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)

    def send_hand(self, vx, vy):
        send_list = [0xAA, send_dict['Hand']]
        send_list += float2byte(vx)
        send_list += float2byte(vy)

        # add check byte
        check = calc_check_byte(send_list)
        send_list.append(check)

        self.write(send_list)


